#include "jsonread.hpp"
#include <fstream>
#include <filesystem>
#include <cmath>
namespace fs = std::filesystem;
Map_pose::Map_pose(std::string filename)
{
     std::ifstream file(filename); 
    if (!file.is_open())  {
        std::cerr << "Error: Failed to open JSON file!" << std::endl;
        return ;
    }
         // 步骤2：解析JSON内容 
    Json::Value root;
    Json::CharReaderBuilder readerBuilder;
    std::string parseErrors;
       // 新版JsonCpp解析方法（推荐）
    bool parsingSuccessful = Json::parseFromStream(
        readerBuilder, file, &root, &parseErrors
    );
    file.close();
    map_name_list.clear();
    for (const auto& item : root["map_name"]) {
         if (item.isString())  {
            map_name_list.push_back(item.asString());
         }
        }
    index_sum = map_name_list.size();
    change_flag=0;
    current_index =0;    
}

void Map_pose::read_file_data(void)
{
    std::string name;
    filepath = fs::current_path();
    //current path=/home/orangepi/laser_processor
    for(int i=0;i<index_sum;++i)
    {
        name = filepath+"/map/"+map_name_list[i]+".json";
        std::ifstream file(name);
           if (!file.is_open())  {
        std::cerr << "Error: file is not open" <<name<< std::endl;
        return ;
                                    }
               // 步骤2：解析JSON内容 
        Json::Value root;
        Json::CharReaderBuilder readerBuilder;
        std::string parseErrors;
        // 新版JsonCpp解析方法（推荐）
        bool parsingSuccessful = Json::parseFromStream(
            readerBuilder, file, &root, &parseErrors
        );
        file.close();
        ////解析数据 
        Shift_data temp_sfd;
            temp_sfd.angle = root["seat0_angle"].asDouble();
            temp_sfd.x = root["seat0_x"].asDouble();
            temp_sfd.y = root["seat0_y"].asDouble();  
        map_seat.push_back(temp_sfd);    
     //   std::cout<<name<<"  angle="<<temp_sfd.angle<<" x="<<temp_sfd.x<<" y="<<temp_sfd.y<<std::endl;       
    }
}

void Map_pose::just_map(void)
{
    std::vector<double>distance;
    for(int i=0;i<index_sum;++i)
    {
       double tempx,tempy,ds;
       tempx= map_seat[i].x-(current_seat_data.x+global_data.x);
       tempy= map_seat[i].y-(current_seat_data.y+global_data.y);
       ds=sqrt(tempx*tempx+tempy*tempy);
       distance.push_back(ds);
    }
        // 遍历比较（时间复杂度O(n)）
    double min_val=distance[0];    
    int min_index=0;
    for(int i = 0; i < distance.size();  ++i)
    {
        if(distance[i] < min_val) {
            min_val = distance[i];
            min_index = i;
        }
    }
    //change_flag =min_index;
    if(current_index!=min_index)
        {
            if((distance[min_index]+0.1)<distance[current_index])
            {
            change_flag=1;
            std::cout<<"distance "<<distance[current_index]<<" new= "<<distance[min_index]<<std::endl;
            current_index=min_index;
            
            }
        }
}

void Map_pose::create_pcd_filename(void)
{
    filepath = fs::current_path();
    cur_pcd_filename = filepath+"/map/"+map_name_list[current_index]+".pcd";
    change_flag=0;
}



